#include <cv.h>
#include <highgui.h>
#include <stdio.h>

#define BIN_THRES 72
#define MIN_AREA 2000 //about 45 by 45 pixels
#define CONFIDENCE_THRES 0.7

// TODO: blob analysis and position estimation (idk if it's just x and y)
// Note: Mat stores images in BGR order

//the following vars are global just for debug output
cv::Mat origImg;
cv::Mat binImg;

cv::Mat getBinaryImage(cv::Mat img, double thres) {
	//the binarization itself doesn't actually convert the image to greyscale, so it has to be done by hand
	cv::Mat tmpGreyImg;
	cv::cvtColor(img, tmpGreyImg, CV_BGR2GRAY);

	//I assume potential ellipses should be white, hence the _INV
	cv::threshold(tmpGreyImg, binImg, thres, 255, cv::THRESH_BINARY_INV);
	return binImg;
}

void findEllipses(cv::Mat original) {
	cv::Mat binImg = getBinaryImage(original, BIN_THRES);

	std::vector<std::vector<cv::Point> > contours;
	//copy since findContours modifies original image
	cv::Mat binImgCopy(binImg.clone());
	cv::findContours(binImgCopy, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
	//cv::drawContours(origImg, contours, -1, cv::Scalar(255, 0, 0, 0));

	for (uint i = 0; i < contours.size(); i++) {
		cv::Mat contourMat(contours[i]);
		//TODO: perform a blob analysis for each contour here?
		cv::Rect bounds = cv::boundingRect(contourMat);
		double area = cv::contourArea(contourMat);
		double circ = cv::arcLength(contourMat, true);
		//the following actually results in a much higher confidence for ACTUAL ellipses, but i'm not sure if it's correct
		//double circ = contours[i].size();
		double compactness = circ * circ / (4 * M_PI * area);

		cv::Mat roi(binImg, bounds);
		cv::Moments m = cv::moments(roi, true);

		double x = m.m10/m.m00;
		double y = m.m01/m.m00;

		double a = m.m20/m.m00 - x * x;
		double b = 2 * (m.m11/m.m00 - x * y);
		double c = m.m02/m.m00 - y * y;

		//divison by 2 because all formulas use the radius
		double l = sqrt(6 * (a + c - sqrt(b*b + (a-c)*(a-c)))) / 2;
		double w = sqrt(6 * (a + c + sqrt(b*b + (a-c)*(a-c)))) / 2;

		double calcArea = M_PI * l * w;
		//ramanujan approx for circumference
		double calcCirc = M_PI * (3 * (l+w) - sqrt((3*l+w)*(l+3*w)));
		double calcCompact = calcCirc * calcCirc / (4 * M_PI * calcArea); //should be very close to 1

		double confidence = calcCompact / compactness;

		if (area >= MIN_AREA && confidence > CONFIDENCE_THRES) {
			printf("ac: %.1f %.1f, c: %.1f, ac: %.1f %.1f, c: %.1f => confidence: %.2f\n", area, circ, compactness, calcArea, calcCirc, calcCompact, confidence);
			cv::drawContours(origImg, contours, i, cv::Scalar(0, 0, 255, 0));		
		}
	}
}

int main(int argc, char **argv) {
	origImg = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
	// given an IplImage* iimg, build a Mat via: "Mat(iimg)"

	findEllipses(origImg);

	cv::namedWindow("Binary Image", CV_WINDOW_AUTOSIZE);
	cv::imshow("Binary Image", binImg);
	cv::namedWindow("Found Ellipses", CV_WINDOW_AUTOSIZE);
    	cv::imshow("Found Ellipses", origImg);
	
    	cv::waitKey(0);
	return 0;
}
